#include <hidef.h> /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */
#include "Services_QG.h"
#include "SCI_HAL_QG.h"
#include "GPS_Handler.h"
#include "SD_Card_Handler.h"
#include "Ultrasound_Handler.h"
#include "Communications_Handler.h"

extern uint8 u8VSCI_TxBuffer[VSCI_TX_BUFFER_SIZE];

uint8 u8IIC_RequestCounter = 0;
uint8 u8VSCI_RequestCounter = 0;

uint8 u8VSCI_Request = 0;
uint8 u8VSCI_Confirmation = 0;

uint16 u16GPS_RelativeAltitude = 0;  //Plane altitude respect to controller. Zero if negative.

/* Altitude is in centimeters */
union union32bits ultrasound_altitude;

void main(void) 
{       
    EnableInterrupts; /* enable interrupts */ 

    /* Initialize clock source: internal, bus = 8 MHz */
    vfnICS_Configure (); 
    vfnSystemOptionsInit();
    
    vfnGPS_WatchdogInit(); // configurar el "watchdog" de la recepcion serial
    vfnGPS_ZeroCoordInit();  //Set the default value for controller coordinates
    vfnGPS_DataProcessing();
    
    /* Initialize sci module, 38400 baud, rx and tx enabled */
    /* Rx interrupts enabled, rest are default */

    vfnSCI_SetBaudRate (SCI_4800BAUD);
    vfnSCI_Init ();
    
    vfnSCI_DisableRxInt();
    vfnGPS_Init ();
    
    vfnSCI_SetBaudRate (SCI_38400BAUD);
    vfnGPS_Init ();
    
    (void)u8SCI_RxPending();
    (void)u8SCI_ReadByte();
    
    vfnSCI_EnableRxInt();
        
    //vfnIIC_Init();
    
    //vfnComm_BuildIICPacket();
    
    vfnVSCI_Configure();
    
    vfnComm_BuildVSCIPacket();
         
    for(;;) 
    {
    	uint8 u8loop_counter;
    		
    	// While the RX pin is held high, wait
		while(VSCI_RX_PIN)
		{
			__RESET_WATCHDOG();
		}
		
		// As soon as the Rx pin goes low, read vsci byte
		
		u8VSCI_Request = u8VSCI_ReadByte();
		
		//if(u8VSCI_Request == 0)
			//u8VSCI_Request = VSCI_GPS_DATA_REQUEST;
		
		// If a data request was received, send packet
		if (u8VSCI_Request == VSCI_GPS_DATA_REQUEST)
		{
			for (u8loop_counter = 0; u8loop_counter < VSCI_TX_BUFFER_SIZE; u8loop_counter++)
			{
				vfnVSCI_SendByte(u8VSCI_TxBuffer[u8loop_counter]);
			}
			
			while(VSCI_RX_PIN);
			
			// Make sure confirmation checksums match
			u8VSCI_Confirmation = u8VSCI_ReadByte();
			
			if (u8VSCI_Confirmation == u8VSCI_TxBuffer[VSCI_TX_BUFFER_SIZE - 1])
			{
				u8VSCI_RequestCounter++;
			}
					
		}
		// if a home request was receieved, set new gps home coordinates
		else if (u8VSCI_Request == VSCI_GPS_HOME_REQUEST)
		{
			vfnGPS_SetHome();
			
			vfnVSCI_SendByte(VSCI_GPS_HOME_FEEDBACK);			
		}
		
		if (u8VSCI_RequestCounter & 0x01)
		{
			vfnGPS_RequestPacket(PACKET_REQUEST_GGA);	
		}
		else
		{
			vfnGPS_RequestPacket(PACKET_REQUEST_VTG);	
		}

    
    	//u16software_delay = 0xFFFF;
    	//while(u16software_delay--);	
    
        __RESET_WATCHDOG(); /* feeds the dog */
        
    } /* loop forever */
  /* please make sure that you never leave main */
}
